#pragma once
// #autor:yxj
#include "rclcpp/rclcpp.hpp"
#include <agv_peripheral_controller/roller/motor_control.h>
#include <agv_peripheral_controller/roller/roller_signal.h>
#include <agv_peripheral_controller/roller/barrier_control.h>
#include <agv_msgs/msg/agv_event_status.hpp>
namespace peripheral_roller
{
    ////////////////参数列表字段//////////////////////
    // 地面辊道的站点类型/动力类型
    enum class StationType
    {
        UNKNOWN, // 未知的
        DYN,     // 有动力辊道
        STATIC   // 无动力辊道
                 // CLOCKWISE_ROTATION,
    };

    // 对接边缘类型
    enum class SideType
    {
        UNKNOWN, // 未知的
        Front,   // 前
        Back,    // 后
        Left,    // 左
        Right    // 右
    };

    // 设备编号
    // 参数列表
    struct CParameters
    {
        // 对接站点类型
        StationType station;
        SideType side;
        std::vector<int> idList;
        CParameters()
        {
            clear();
        }
        void clear()
        {
            station = StationType::UNKNOWN;
            side = SideType::UNKNOWN;
            idList.clear();
        }
    };

    // DT  正常通信
    // 地面辊道信号-floor -> agv
    // IN1	卸物料正常
    // IN2	接物料正常
    // IN3	辊道运行中
    // IN4	接料完成
    // 地面信号
    enum FloorSig
    {
        DropToAgvReady = 0,
        PickFromAgvReady,
        FloorRollerRunning,
        PickFromAgvFinish
    };

    // agv辊道信号-agv -> floor
    // OUT1	卸物料正常
    // OUT2	接物料正常
    // OUT3	辊道运行中
    // OUT4	接料完成
    enum AgvSig
    {
        DropToFloorReady = 0,
        PickFromFloorReady,
        AgvRollerRunning,
        PickFromFloorFinish
    };

    // agv接料阶段
    enum class PickStage
    {
        AgvReady = 0,    // agv接料正常就绪
        FloorReady,      // 地面辊道卸料正常就绪
        AgvFloorRunning, // agv辊道运行+地面辊道运行
        DelayFinish,     // 延时停止辊筒
        IDLE,            // 空闲
        ForcePick,       // 一键强制取货
        ForcePause,      // 一键强制暂停
        Unkown           // 未知
    };

    // agv接料阶段
    enum class DropStage
    {
        AgvReady = 0,    // agv卸料正常就绪
        FloorReady,      // 地面辊道接料正常就绪
        AgvFloorRunning, // 地面辊道运行 +agv辊道运行
        IDLE,            // 空闲
        ForceDrop,       // 一键强制卸货
        ForceDropDelay,  // 一键强制卸货延迟
        ForcePause,      // 一键强制暂停
        Unkown           // 未知
    };

    // 自动运行时报警
    enum class AutoRunErr
    {
        No,            // 无报警
        FloorStop,     // 地面辊筒异常停止
        FloorAbnormal, // 地面辊道normal信号异常消失
        DtMiss         // 光通讯模块对接失败

    };

    // 货物开关触发控制电机运行速度
    enum class CargoBitState
    {
        High,
        Low,
        Stop
    };

    // 辊筒电机正反向，高低速
    struct CMotorVel
    {
        float forwardHighVel; // 正向高低速-带正负号
        float forwardLowVel;
        float backWardHighVel; // 反向高低速-带正负号
        float backWardLowVel;
        float delayFinishPicKTime; // 自动取货完成 ms
        bool openOneClickAction;   // 是否开启一键取送货功能
        float endPickDis;          // 取货时，最后到位继续运行的距离(m)
        CMotorVel()
        {
            forwardHighVel = forwardLowVel = 0.0;
            backWardHighVel = backWardLowVel = 0.0;
            delayFinishPicKTime = 5000;
            openOneClickAction = false;
            endPickDis = 0.1;
        }
        void print()
        {
            ROS_INFO_STREAM_THROTTLE(1, "forward:HighVel=" << forwardHighVel << ",LowVe=" << forwardLowVel
                                                           << ",backWard:HighVel=" << backWardHighVel
                                                           << ",LowVel=" << backWardLowVel
                                                           << ",delayFinishPicKTime=" << delayFinishPicKTime
                                                           << ",openOneClickAction=" << openOneClickAction
                                                           << ",endPickDis=" << endPickDis

            );
        }
    };

    // IO举升控制类
    class CRollerController
    {
    public:
        // 默认构造函数
        CRollerController();

        // 析构函数
        ~CRollerController();

        // 是否初始化完成
        bool initFinish();

        // 初始化函数
        bool init();

        // 取货
        bool pick();

        // 卸货
        bool drop();

        // 停止
        bool stop();

        // 复位状态
        void resetAutoStage();

        // 准备开始-停止设备，复位开始阶段
        // 复位所有信号
        bool readyStart(CParameters params = CParameters());

        // 货物装填
        bool loadStatus();

        //   状态监控返回当前的事件控制模式 --是否是急停事件
        bool emgEventOccur();

        // 设备是否报错
        bool fault();

        // 报错信息
        std::string faultMsg();

        // 按下复位按钮清错一次
        bool clearFault();

        // 读取辊筒电机的真实运行状态
        bool setRollerMotorOcSigal(bool run);

        // 手动控制逻辑
        void manulControl();

        // 辊筒电机信息
        std::vector<agv_msgs::msg::PeriphMotorInfo> periphMotor();

        // 当前步骤-发送外设信息
        int curStep();

    private:
        // 订阅事件话题-回调函数
        void eventCB(const agv_msgs::msg::AGVEventStatus &event);

        // 辊筒电机速度参数加载
        bool readMotorVelParam();

        // 阻挡手动控制
        void barrierManul();

        // 辊筒电机手动控制
        void motorManul();

        // 业务逻辑报警-地面辊道异常停止等错误
        bool autoRunFault();

        // 清除自动运行错误
        void clearAutoRunFault();

        // 异常消息
        std::string autoRunFaultMsg();

        // 货物开关的状态
        CargoBitState cargoBits();

        // 仿真信号输出
        void simCycle();

        // 检测是否强制跳转一键取货
        bool checkForcePick();

        // 检测是否强制跳转一键卸货
        bool checkForceDrop();

        // 清除光通讯输出
        bool clearOc();

    private:
        /////////////////// pick////////////////
        // agv就绪
        bool agvReadyForPick();

        // 地面就绪
        bool floorReadyForPick();

        // agv辊道运行+ 地面辊道运行
        bool agvAndFloorRunForPick();

        // 延时停辊筒
        bool delayFinishForPick();

        //////////////一键取货//////////////
        // 取货时，电机速度阶梯
        CargoBitState stepVelPick(float &fRunVel);

        // 一键取货-忽略对面完成信号->finishForPick
        bool oneClickPick();

        ///////////////// drop//////////////////
        // agv就绪
        bool agvReadyForDrop();

        // 地面就绪
        bool floorReadyForDrop();

        // agv辊道运行 +地面辊道运行
        bool agvAndFloorRunForDrop();

        /////////////////一键卸货///////////////
        // 延迟停止卸货完成
        bool delayFinishDrop();

        // 卸货时，电机速度阶梯
        CargoBitState stepVelDrop();

        // 一键卸货-忽略对面完成信号
        bool oneClickDrop();

    private:
        // 初始化完成标识
        bool m_init;

        //取货完成后，最后补偿的距离累加值
        float m_curDistance;

        // 当前动作名称-取货还是送货
        std::string m_actionName;

        // 私有节点句柄
        rclcpp::Node m_priNh;

        // 订阅事件话题
        ros::Subscriber m_subEvent;

        // 定时器2s延迟
        CTimerTriggerOnce m_timer;

        // 定时器延迟
        CTimerTriggerOnce m_timerDelay;

        // 获取的事件消息
        agv_msgs::msg::AGVEventStatus m_eventStatus;

        // 前阻挡
        CBarrierControl m_barrier;

        // 辊筒电机
        CMotorControl m_rollerMotor;

        // 辊筒-货物检测和光通信等信号
        CRollerSignal m_signals;

        // 取货-接料阶段
        PickStage m_pick;

        // 送货-送料
        DropStage m_drop;

        // 辊筒电机速度参数
        CMotorVel m_motorParam;

        // 自动运行时，业务流程报警
        AutoRunErr m_runFault;

        // 参数列表
        CParameters m_params;
    };
}